"""  
需求：
流程：
    1.导包；
    2.初始化ROS2客户端；
    3.自定义节点类；
                  
    4.调用spain函数，并传入节点对象；
    5.资源释放。


"""
#1.导包；
import rclpy
from rclpy.node import Node
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from geometry_msgs.msg import Twist
from rclpy.time import Time
import math
#3.自定义节点类；
class Exer03TFListenerPy(Node):
    def __init__(self):
        super().__init__("exer03_tf_listener_py_node")
        self.declare_parameter("father_frame","turtle2")
        self.declare_parameter("child_frame","turtle1")
        self.father_frame=self.get_parameter("father_frame").get_parameter_value().string_value
        self.child_frame=self.get_parameter("child_frame").get_parameter_value().string_value
        self.buffer=Buffer()
        self.listener=TransformListener(self.buffer,self)
        self.cmd_pub=self.create_publisher(Twist,self.father_frame+"/cmd_vel",10)
        self.timer=self.create_timer(1.0,self.on_timer)
    def on_timer(self):
        if self.buffer.can_transform(self.father_frame,self.child_frame,Time()):
            ts=self.buffer.lookup_transform(self.father_frame,self.child_frame,Time())
            twist=Twist()
            twist.linear.x=0.5*math.sqrt(math.pow(ts.transform.translation.x,2)+math.pow(ts.transform.translation.y,2))
            twist.angular.z=1.0*math.atan2(ts.transform.translation.y,ts.transform.translation.x)
            self.cmd_pub.publish(twist)
def main():
    #2.初始化ROS2客户端；
    rclpy.init()
    #4.调用spain函数，并传入节点对象；
    rclpy.spin(Exer03TFListenerPy())
    #5.资源释放。
    rclpy.shutdown()

if __name__=='__main__':
    main() 